#include "modules/MonitorModule/FrontPushableObsMonitor.h"
#include "modules/CtrlModule/PassivePlanner/FrontPushableObsPlanner.h"
#include "data_struct/base/DataSet.h"
#include "modules/CtrlModule/MarkPosePubModel.h"

namespace  behavior_controller
{
    FrontPushableObsMonitor front_pushable_obs_monitor;
    void FrontPushableObsMonitor::run()
    {
        if(front_pushable_obs_planner.finished)
        {
            if(DataSet::object_data.GetPosesArray().size()>0)
            {   
                std::vector<PointF> tmp_world_front_obs_pose;
                PointF robot_pose_tmp;
                double x=0,y=0;
                bool find_similar=false;

                for(int i=0; i<DataSet::object_data.GetPosesArray().size(); i++)
                {
                    robot_pose_tmp= DataSet::robot_pose.ConvertOnCur(
                                camera_pose.ConvertOnCur(DataSet::object_data.GetPosesArray()[i]) );
                    x+= robot_pose_tmp.x();
                    y+= robot_pose_tmp.y();
                    tmp_world_front_obs_pose.emplace_back(robot_pose_tmp);
                }
                x/=DataSet::object_data.GetPosesArray().size();
                y/=DataSet::object_data.GetPosesArray().size();
                for(int i=0; i< front_pushable_obs_planner.front_obs.size();i++)
                {                        
                    if (front_pushable_obs_planner.front_obs[i].second.Distance(PointF(x,y)) < 2*ROBOT_RADIUS
                        && front_pushable_obs_planner.front_obs[i].first)
                    {
                        find_similar=true;
                        break;
                    }   
                }

                if(!find_similar)
                {
                    front_pushable_obs_planner.front_obs.push_back(std::make_pair(false,PointF(x,y)));             
                    front_pushable_obs_planner.finished=false;
                    front_pushable_obs_planner.push_able= DataSet::object_data.PushAble();
                    DataSet::ctrl_state = FRONT_PUSHABLE_OBS_PLAN;
                    mark_pose_pub_mod.MarkPosePub(tmp_world_front_obs_pose);
                    std::cout<< "front_pushable_obs  find !"<< std::endl;
                }
            }
        }
    }
}